#include"pch.h"
///進(jìn)行一次掃描,轉(zhuǎn)換為大地坐標(biāo)系,并作為參考數(shù)據(jù)
if(!m_client->scan_to_reference())//把基于大地坐標(biāo)系的掃描數(shù)據(jù)放到參考窗口
return;
//刪除底面
//delete_plane(m_gun_PlaneInfo);
bin::vector<techlego::pos6f>plane_points1;
bin::vector_h<techlego::h_point_info>pt_clouds1;
m_client->get_scan_data_by_index(pt_clouds1,0);
for(int i=0;i<pt_clouds1.m_size;i++)
{
bin::vector<double>pt;
techlego::pos6f pos6f;
pt_clouds1[i].get_point_info(pt);
pos6f.m_x=pt[0];
pos6f.m_y=pt[1];
pos6f.m_z=pt[2];
pos6f.m_nx=pt[3];
pos6f.m_ny=pt[4];
pos6f.m_nz=pt[5];
plane_points1.emplace_back(pos6f);
}
//把當(dāng)前點(diǎn)作為平面上的點(diǎn),點(diǎn)的法相作為平面的法向,刪除點(diǎn)云(刪除底面)
plane.filter_points_by_plane(plane_points1,-500,filter_height);
m_client->load_feature_align_data_from_memory(plane_points1,true);
//定義機(jī)器手移動位姿
techlego::pose_3d res;
techlego::pose_3d res1;
double error;
bin::vector<int>inde;
///調(diào)用feature_align
if(!m_client->feature_align(res,error,inde))//將移動數(shù)據(jù)移動到與參考數(shù)據(jù)重合,并得到移動位姿
{
if(filter_height==filter_height_max)
{
BCGPMessageBox(m_hWnd,L"未識別到目標(biāo)!\n開始識別下一層",L"提示",MB_OK|MB_ICONHAND|MB_ICONASTERISK);
filter_height=3;
continue;
}
if(filter_height==filter_height_min)
{
BCGPMessageBox(m_hWnd,L"未識別到目標(biāo)!停止掃描",L"提示",MB_OK|MB_ICONHAND|MB_ICONASTERISK);
break;
}
}